#include "data_reflow/event_subscriber.h"

namespace data_reflow
{
    EventSubscriber::EventSubscriber() : Node("test_event_subscriber_node")
    {
        // 1. 初始化事件订阅者
        event_sub_ = this->create_subscription<data_reflow_interfaces::msg::GetEvent>(
            "event_topic", 10,
            std::bind(&EventSubscriber::event_callback, this, std::placeholders::_1));

        // 2. 初始化摄像头数据订阅者
        camera_sub_ = this->create_subscription<data_reflow_interfaces::msg::SensorData>(
            "camera_data_pub", 10, // 与数据发布节点的话题名保持一致
            std::bind(&EventSubscriber::camera_data_callback, this, std::placeholders::_1));

        RCLCPP_INFO(this->get_logger(), "事件订阅者节点已启动，等待碰撞事件...");

        // 新增计数器
        count_timer_ = this->create_wall_timer(
            std::chrono::seconds(5),
            [this]()
            {
                RCLCPP_INFO(this->get_logger(), "订阅端累计接收摄像头帧数：%u", camera_receive_count_);
                // 同时打印 DataBuffer 中当前缓存的帧数，对比是否一致
                auto &buffer = DataBuffer::get_instance();
                auto cached_count = buffer.getAllData(SensorType::TYPE_CAMERA).size();
                RCLCPP_INFO(this->get_logger(), "DataBuffer 中缓存的摄像头帧数：%lu", cached_count);
            });
    }

    // 3. 实现摄像头数据回调函数（缓存数据到DataBuffer）
    void EventSubscriber::camera_data_callback(const data_reflow_interfaces::msg::SensorData::SharedPtr msg)
    {
        if (msg)
        {
            camera_receive_count_++; // 新增：每收到一帧递增计数
            RCLCPP_DEBUG(this->get_logger(), "收到摄像头帧：计数%d，时间戳%d.%09u",
                         msg->count, msg->stamp.sec, msg->stamp.nanosec);
            DataBuffer::get_instance().addData(*msg); // 缓存数据
        }
    }

    void EventSubscriber::event_callback(const data_reflow_interfaces::msg::GetEvent::SharedPtr msg)
    {
        if (msg->event_type == "collision")
        {
            RCLCPP_INFO(this->get_logger(), "收到碰撞事件: %s, 时间戳: %d.%09u",
                        msg->description.c_str(), msg->stamp.sec, msg->stamp.nanosec);
            handle_collision_event(msg->stamp);
        }
        else
        {
            RCLCPP_DEBUG(this->get_logger(), "收到非碰撞事件：%s，忽略", msg->event_type.c_str());
        }
    }

    void EventSubscriber::handle_collision_event(const builtin_interfaces::msg::Time &event_stamp)
    {
        // 1. 保存碰撞事件时间和查询范围（用成员变量存储，供定时器回调使用）
        event_time_ = rclcpp::Time(event_stamp);
        start_time_ = event_time_ - rclcpp::Duration::from_seconds(5);
        end_time_ = event_time_ + rclcpp::Duration::from_seconds(5);

        // 2. 打印当前信息，启动5秒后查询的定时器（非阻塞）
        RCLCPP_INFO(this->get_logger(), "碰撞事件时间：%f（秒）", event_time_.seconds());
        RCLCPP_INFO(this->get_logger(), "将在5秒后查询时间范围：[%f, %f]（秒）",
                    start_time_.seconds(), end_time_.seconds());
        RCLCPP_INFO(this->get_logger(), "延迟期间正常接收新数据...");

        // 3. 创建一次性定时器（5秒后执行查询，执行后自动销毁）
        query_timer_ = this->create_wall_timer(
            std::chrono::seconds(5),
            [this]()
            {
                // 5秒后执行的查询逻辑（此时线程已释放，期间已接收新数据）
                auto &buffer = DataBuffer::get_instance();

                // 打印缓存最新状态（确认包含后5秒数据）
                auto all_cached = buffer.getAllData(SensorType::TYPE_CAMERA);
                if (!all_cached.empty())
                {
                    rclcpp::Time cached_min_time(all_cached.front().stamp);
                    rclcpp::Time cached_max_time(all_cached.back().stamp);
                    RCLCPP_INFO(this->get_logger(), "5秒后缓存状态：时间范围[%f, %f]，共%lu帧",
                                cached_min_time.seconds(), cached_max_time.seconds(), all_cached.size());
                }

                // 执行查询
                auto camera_data = buffer.queryByTypeAndTime(
                    SensorType::TYPE_CAMERA, start_time_, end_time_);

                // 后续查询结果打印逻辑（不变）
                if (camera_data.empty())
                {
                    RCLCPP_WARN(this->get_logger(), "未查询到碰撞前后5秒的摄像头数据（%f ~ %f）",
                                start_time_.seconds(), end_time_.seconds());
                    return;
                }
                RCLCPP_INFO(this->get_logger(), "成功查询到 %lu 帧摄像头数据（%f ~ %f）",
                            static_cast<unsigned long>(camera_data.size()),
                            start_time_.seconds(), end_time_.seconds());
                for (size_t i = 0; i < std::min(camera_data.size(), 3UL); ++i)
                {
                    const auto &data = camera_data[i];
                    RCLCPP_INFO(this->get_logger(), "第%lu帧：计数%u，时间戳%d.%09u，数据：%s",
                                static_cast<unsigned long>(i + 1), data.count,
                                data.stamp.sec, data.stamp.nanosec, data.camera_data.c_str());
                }

                // 定时器执行后手动停止（避免重复触发）
                query_timer_->cancel();
            });
    }
} // namespace data_reflow